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ABSTRACT 

Recent  technological  advances  have  significantly  im¬ 
proved  the  capabilities  of  micro-air  vehicles  (MAV). 
This  is  evidenced  by  their  expanding  use  by  govern¬ 
ment,  police,  and  military  forces.  A  MAV  can  provide 
a  real-time  surveillance  capability  to  even  the  smallest 
units,  which  provides  commanders  with  a  significant 
advantage. 

This  capability  is  a  result  of  the  availability  of  minia¬ 
turized  autopilot  systems  which  typically  combine  in¬ 
ertial,  pitot-static,  and  GPS  sensors  into  a  feedback 
flight-control  system.  While  these  autopilots  can  pro¬ 
vide  an  autonomous  flight  capability,  they  have  some 
limitations  which  impact  their  operational  effective¬ 
ness.  One  of  the  primary  issues  is  poor  image  geolo¬ 
cation  performance,  which  limits  the  use  of  these  sys¬ 
tems  for  direct  measurements  of  target  locations.  This 
poor  geolocation  performance  is  primarily  a  conse¬ 
quence  of  the  relatively  large  attitude  errors  charac¬ 
teristic  of  low-performance  inertial  sensors.  In  pre¬ 
vious  efforts,  we  have  developed  a  tightly-coupled 
image-aided  inertial  navigation  system  to  operate  in 
areas  not  serviced  by  GPS.  This  system  extracts  nav¬ 
igation  information  by  automatically  detecting  and 
tracking  stationary  optical  features  of  opportunity  in 
the  environment.  One  characteristic  of  this  system  is 
vastly  reduced  attitude  errors,  even  with  consumer- 
grade  inertial  sensors. 

In  this  paper,  the  benefits  of  incorporating  image- 
based  navigation  techniques  with  inertial  and  GPS 
measurements  is  explored.  After  properly  integrat¬ 
ing  GPS  with  the  image-aided  inertial  architecture. 
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the  system  is  tested  using  a  combination  of  Monte- 
Carlo  simulation  and  flight  test  data.  The  flight  test 
data  was  flown  over  Edwards  AFB  using  represen¬ 
tative  hardware.  The  experimental  results  are  com¬ 
pared  with  validated  truth  data.  The  effects  of  vari¬ 
ations  in  sensor  quality  and  integration  methods  are 
investigated  and  shown  to  greatly  improve  the  over¬ 
all  performance  of  the  tightly-coupled  image-aided 
sensor  over  the  reference  GPS /INS  sensor. 

INTRODUCTION 

Virtually  all  UAV s  and  aircraft  are  flying  with  electro- 
optical  (EO),  infrared  (IR),  or  video  cameras.  Even 
the  smallest  can  now  be  equipped  with  an  integrated 
Global  Positioning  System  (GPS)  and  inertial  naviga¬ 
tion  system  (INS).  Unfortunately,  there  is  a  very  lim¬ 
ited  set  of  vehicles  that  provide  the  level  of  navigation 
and  timing  precision  to  the  imaging  path  sufficient  for 
generating  high-quality  geospatial-intelligence  prod¬ 
ucts.  This  is  due  to  a  variety  of  reasons  such  as  cost 
or  payload  restrictions  that  mandate  small  low  cost 
inertial  navigation  systems;  or,  to  difficulties  in  pre¬ 
cise  timing  of  the  metadata  to  the  epoch  of  the  frame 
exposure;  or,  to  the  expense  and  complications  in¬ 
volved  with  establishing  accurate  GPS  positions  us¬ 
ing  dual  frequency  phase  difference  processing  with 
associated  communications  links. 

In  order  to  address  the  issue  of  poor  georegistration 
performance  for  small  uavs,  a  number  of  research 
projects  have  been  conducted.  The  most  prominent 
approaches  address  the  issue  in  three  primary  ways. 
The  first,  and  probably  most  obvious,  approach  is  to 
simply  improve  the  quality  of  the  inertial  and  GPS 
sensors  until  the  desired  target  location  accuracy  is 
achieved.  The  advantage  of  this  approach  is  its  sim¬ 
plicity.  The  disadvantages  are  the  increased  cost  and 
weight  requirements  for  improved  sensors  (especially 
gyroscopes)  that  make  this  approach  of  limited  ap¬ 
peal  for  small  UAV  operations.  A  variation  on  this 
theme  was  proposed  by  Hirokawa  [5]  where  three 
very  low-cost  GPS  sensors  with  displaced  antennas 
were  used  to  greatly  improve  the  attitude  perfor¬ 
mance  of  the  navigation  sensor,  resulting  in  improved 
geolocation  accuracy.  The  second  approach  [1]  uti¬ 
lizes  previously-surveyed  reference  targets  in  the  im¬ 
ages  to  automatically  update  and  correct  the  naviga¬ 
tion  errors  in  the  UAV.  This  system  has  the  capability 
to  eliminate  inertial  sensor  drift  when  the  aircraft  re¬ 
mains  in  an  operations  area  with  visible  reference  tar¬ 
gets.  The  final  approach  uses  post-flight  image  reg¬ 
istration  software  to  correct  each  image  to  a  known 
set  of  ground  control  points.  This  process  can  be  per¬ 
formed  manually  or  automatically  and  tends  to  be 


used  in  an  open-loop,  postflight  mode  (i.e.,  naviga¬ 
tion  error  estimates  are  not  fed  back  to  the  vehicle  for 
correction). 

The  motivation  for  this  paper  is  to  determine  if  the 
position  and  orientation  of  camera  frames  can  be  sig¬ 
nificantly  improved  by  augmenting  the  camera  nav¬ 
igation  system  with  an  image-aided  algorithm  that 
reliably  tracks  image  features  across  frames  in  a  ro¬ 
bust  manner.  This  implies  that  the  image-aiding  al¬ 
gorithms  add  to  the  solution  across  a  wide  variety 
of  terrain  types,  thus  allowing  for  additional  esti¬ 
mates  of  the  camera  position  and  orientation  in  the 
dynamic  adjustment.  As  an  additional  benefit,  the 
system  should  be  able  to  continue  to  navigate  in  the 
presence  of  GPS  dropouts. 

The  proof  of  concept  testing  will  involve  both  simu¬ 
lations  and  generation  of  accuracy  estimates  that  will 
conclude  with  testing  a  MAV  prototype  flown  over  a 
test  area  with  visible  survey  control  points  that  will 
be  compared  to  derived  coordinates  from  the  sys¬ 
tem.  Additionally,  Geo-TIFFs  will  be  generated  from 
the  camera/MAV  system  and  compared  to  control. 
The  overall  goal  of  the  project  is  to  deliver  a  proto¬ 
type  small-UAV  imagery  collection  system  to  the  Na¬ 
tional  Geospatial-Intelligence  Agency  (NGA)  for  fur¬ 
ther  testing  and  use  in  generating  Geo-Intelligence 
and  Geo-Security  products.  The  purpose  of  this  pa¬ 
per  is  to  outline  the  method  used  to  fuse  the  image, 
inertial,  and  GPS  measurements  with  a  simple  terrain 
database.  The  algorithm  is  tested  using  Monte  Carlo 
simulation  and  experimental  flight  data  collected  us¬ 
ing  prototype  hardware.  Results  and  lessons  learned 
will  be  addressed  and  incorporated  into  future  de¬ 
signs. 

This  paper  is  organized  as  follows.  First,  a  develop¬ 
ment  of  the  assumptions  and  sensor  models  regard¬ 
ing  the  specific  problem  of  interest  is  presented.  In 
addition,  the  extended  Kalman  filter  used  to  tightly- 
couple  the  image  tracking  and  GPS /INS  functionality 
is  described.  Next,  the  performance  of  the  integrated 
navigation  technique  is  compared  to  the  stand-alone 
GPS/INS  technique  using  a  Monte-Carlo  simulation 
and  flight  test  data.  Finally,  conclusions  are  drawn  re¬ 
garding  the  performance  of  the  technique  and  future 
work  is  presented. 

DEVELOPMENT 

As  mentioned  in  the  previous  section,  the  goal  of  this 
research  effort  is  to  investigate  the  navigation  and 
target  location  accuracy  improvements  achievable  by 
tightly  integrating  an  image-based  feature  tracking 
algorithm  with  GPS  and  a  consumer-grade  inertial 


sensor.  In  this  section,  the  overall  design  of  the  ex¬ 
tended  Kalman  filter  [8, 9]  is  presented  along  with  a 
description  of  the  sensor  models. 

Assumptions 

This  method  is  based  on  the  following  assumptions. 

•  A  strapdown  inertial  measurement  unit  (IMU) 
and  GPS  antenna  are  rigidly  attached  to  one 
or  more  calibrated  cameras.  Synchronized  raw 
measurements  are  available  from  all  sensors. 

•  The  inertial,  GPS  and  optical  sensors'  relative  po¬ 
sition  and  orientation  are  known  (see  [13]  for  a 
discussion  of  boresight  calibration  procedures). 

•  The  camera  images  areas  in  the  environment 
which  contain  some  stationary  objects. 

•  A  statistical  terrain  model  is  available  which  pro¬ 
vides  an  initial  indication  of  range  to  objects  in 
the  environment. 


Reference  Frames 

In  this  paper,  four  reference  frames  are  used.  Vari¬ 
ables  expressed  in  a  specific  reference  frame  are  indi¬ 
cated  using  superscript  notation.  The  Earth-Centered 
Earth-Fixed  (ECEF,  or  e  frame)  is  a  Cartesian  system 
with  the  origin  at  the  Earth's  center,  the  xe  axis  point¬ 
ing  toward  the  intersection  of  the  equator  and  the 
prime  (Greenwich)  meridian,  the  ze  axis  extending 
through  the  North  pole,  and  the  ye  axis  is  the  orthog¬ 
onal  compliment  (in  this  paper,  a  carat  symbol,  ",  de¬ 
notes  a  unit  vector). 

The  Earth-fixed  navigation  frame  (rc-frame)  is  an  or¬ 
thonormal  basis  in  5ft3,  with  origin  located  at  a  pre¬ 
defined  location  on  the  Earth,  typically  very  close  to 
the  current  operations  area.  The  Earth-fixed  naviga¬ 
tion  frame's  x,  y,  and  z  axes  point  in  the  north,  east, 
and  down  (NED)  directions  relative  to  the  origin,  re¬ 
spectively.  Down  is  defined  as  the  direction  of  the 
nominal  gravity  vector  at  the  origin.  In  contrast  to  the 
body-fixed  navigation  frame,  the  Earth-fixed  naviga¬ 
tion  frame  remains  fixed  to  the  surface  of  the  Earth. 
While  this  frame  is  not  useful  for  very-long  distance 
navigation,  it  can  simplify  the  navigation  kinematic 
equations  for  local  navigation  routes  and  is  especially 
appropriate  for  micro-air  vehicle  flight  profiles.  The 
Earth-fixed  navigation  frame  is  shown  in  Figure  1 . 

The  vehicle  body  frame  (or  b  frame)  is  a  Cartesian  sys¬ 
tem  with  origin  at  the  vehicle  center  of  gravity,  the  xb 


Figure  1:  Earth-fixed  navigation  frame.  The  Earth-fixed 
navigation  frame  is  a  Cartesian  reference  frame  zvhich  is 
perpendicidar  to  the  gravity  vector  at  the  origin  and  fixed 
to  the  Earth. 

axis  extending  through  the  vehicle's  nose,  the  yb  axis 
extending  through  the  vehicle's  right  side,  and  the  zb 
axis  points  orthogonally  out  the  bottom  of  the  vehi¬ 
cle.  The  inertial  measurements  are  expressed  in  the  b 
frame. 

The  camera  frame  (or  c  frame)  is  a  Cartesian  system 
with  origin  at  the  center  of  the  camera  image  plane, 
the  xc  axis  is  parallel  to  the  camera  image  plane  and 
defined  as  "camera  up",  the  yc  axis  is  parallel  to  the 
camera  image  plane  and  defined  as  "camera  right", 
and  the  zc  axis  points  out  of  the  camera  aperture,  or¬ 
thogonal  to  the  image  plane.  The  camera  frame  is 
shown  in  Fig.  2. 


Figure  2:  Camera  frame  illustration.  The  camera  reference 
frame  originates  at  the  optical  center  of  the  camera. 


Algorithm  Description 

The  system  parameters  (see  Table  1)  consist  of  the 
navigation  parameters  (position,  velocity,  and  atti¬ 
tude),  inertial  sensor  biases,  GPS  clock  bias  and  drift, 
and  a  vector  describing  the  location  of  landmarks  of 
interest  (tn)  in  the  navigation  frame.  The  navigation 
parameters  are  calculated  using  body-frame  velocity 
increment  (Avb)  and  angular  increment  (A 0bib)  mea¬ 
surements  from  the  inertial  navigation  sensor  which 
have  been  corrected  for  bias  errors  using  the  current 
filter-computed  bias  estimates.  These  measurements 
are  integrated  from  an  initial  state  in  the  navigation 
(local-level)  frame  using  mechanization  algorithms 
described  in  [12]. 


Table  1:  System  Parameter  Definition 


Parameter 

Description 

pn 

Vehicle  position  in  navigation  frame 
(northing,  easting,  and  down) 

vn 

Vehicle  velocity  in  navigation  frame 
(north,  east,  down) 

/-*n 

'-'ft 

Vehicle  body  to  navigation  frame  DCM 

ab 

Accelerometer  bias  vector 

b6 

Gyroscope  bias  vector 

c 

Location  of  landmark  m  in  the 
navigation  frame  (one  for  each  landmark 
currently  tracked) 

fi^gps 

GPS  clock  bias 

fi^gps 

GPS  clock  drift 

The  position,  velocity,  and  attitude  errors  were  mod¬ 
eled  as  a  stochastic  process  based  on  the  well-known 
Pinson  navigation  error  model  [12].  The  accelerome¬ 
ter  and  gyroscopic  bias  errors  were  each  modeled  as 
a  first-order  Gauss-Markov  process  [8],  based  on  the 
specification  for  the  inertial  measurement  unit  (IMU). 
The  GPS  clock  drift  is  modeled  as  a  random  bias.  The 
landmarks  are  modeled  as  stationary  with  respect  to 
the  Earth.  A  small  amount  of  process  noise  is  added 
to  the  state  dynamics  to  promote  filter  stability  [9]. 

EKF  Mechanization 

Because  both  the  system  dynamics  model  and  mea¬ 
surement  models  are  non-linear  stochastic  differential 
equations,  an  extended  Kalman  filter  algorithm  is  em¬ 
ployed.  While  other  recursive  estimation  techniques 
have  been  proposed  which  have  superior  character¬ 
istics  when  dealing  with  non-linear  models  (e.g.,  un¬ 
scented  Kalman  filters  or  particle  filters  [4]),  the  ex¬ 
tended  Kalman  filter  is  still  widely  used  for  integrat¬ 
ing  inertial  and  GPS  sensors. 


In  this  paper,  the  extended  Kalman  filter  is  an  error- 
state  with  feedback  formulation  which  estimates  the 
errors  about  the  nominal  trajectory  produced  by 
the  nonlinear  filter  dynamics  model.  In  addition, 
this  nominal  trajectory  serves  as  the  operating  point 
where  the  nonlinear  dynamics  and  measurement 
models  are  linearized  [9[.  Finally,  the  feedback  na¬ 
ture  attempts  to  constrain  the  inevitable  departure  of 
the  nominal  trajectory  by  periodically  removing  error 
estimates.  This  feedback  process  improves  the  per¬ 
formance  of  the  extended  Kalman  filter  by  reducing 
linearization  errors  due  to  errors  in  the  nominal  tra¬ 
jectory.  For  more  information  regarding  methods  for 
integrating  GPS  and  inertial  sensors,  see  [11].  A  block 
diagram  of  the  system  is  shown  in  Figure  3. 


Figure  3:  Image-aided  GPS/IMU  navigation  filter  block 
diagram.  In  this  filter,  the  location  of  stationary  objects 
are  tracked  and  used  to  estimate  and  update  the  errors  in  a 
GPS-aided  inertial  navigation  system.  The  inertial  naviga¬ 
tion  system  is,  in  turn,  used  to  support  the  feature  tracking 
loop. 


GPS  Model  and  Updates 

The  navigation  filter  is  based  on  single-frequency 
pseudorange  measurements,  which  can  be  corrected 
using  a  differential  reference  station  at  a  surveyed  lo¬ 
cation.  A  typical  pseudorange  measurement  from  the 
mobile  receiver  to  satellite  k  is  modeled  by 

Pm  =  llPfc  ~  Pro  II  +c5tmgpa  +  Tfj  + +  vm  +  mm  (1) 

where  p[!  is  the  satellite  location  vector  in  the  n-frame, 
p'f  is  the  mobile  receiver  location  vector  in  the  71- 
frame,  c  is  the  speed  of  light,  Stmgpi!  is  the  mobile  re¬ 
ceiver  clock  error,  if,  is  the  tropospheric  code  delay 
from  the  mobile  receiver  to  satellite  k,  if  is  the  iono¬ 
spheric  code  delay  from  the  mobile  receiver  to  satel¬ 
lite  k,  vm  is  the  code  measurement  noise,  and  mm  is 
the  code  multipath. 


The  pseudorange  measurement  from  the  reference 
station  to  satellite  k  is  modeled  similarly  as 

Pr  =  llPfc  Pr  II  +  C^rgpa  +  Tp  +  if  +  Vr  +  mr  (2) 

Because  the  reference  station  is  at  a  known  location, 
the  equation  can  be  rewritten  as 

Pr  -  llPfc  -  Pr  II  =  cStrgpa  +  if  +  if  +  Vr  +  mr  (3) 

Subtracting  (3)  from  (1)  and  grouping  common  error 
terms  yields  the  differentially  corrected  pseudorange 
measurement 

Pcorr  =  II Pfc  —  pr^n\\+cStgpS+ATk+AIk+i'+Am  (4) 

If  the  distance  between  the  reference  and  mobile 
receivers  is  small,  the  differential  atmospheric  and 
satellite  position  errors  become  insignificant.  Group¬ 
ing  the  error  terms  into  a  common  random  variable, 
v,  yields  the  nonlinear  measurement  equation 

Pcorr  =  1 1  Pfc  Pm  II  T  cStgpS  T  V  (5) 

Note  this  is  of  the  general  form 

z  =  h  [x]  +  v  (6) 

where  x  is  the  navigation  state  vector  and  v  is  mod¬ 
eled  as  zero-mean  additive  white  Gaussian  noise  with 
variance 

al=E[S]  (7) 

where  E  [•]  is  the  expectation  operator.  For  this  simu¬ 
lation,  a  variance  of  of,  =  2.6 m2  was  chosen  as  a  con¬ 
servative  estimate  of  the  pseudorange  errors  found  in 
a  consumer-grade,  single-frequency  GPS  receiver.  In 
the  next  section,  the  automatic  feature  detection  and 
tracking  algorithm  is  presented. 

Automated  Feature  Tracking 

Significant  navigation  information  can  be  extracted 
by  tracking  stationary  optical  features  over  multi¬ 
ple  images  [14].  In  order  to  accomplish  this  task, 
a  number  of  issues  must  be  addressed,  namely,  fea¬ 
ture  detection,  feature  correspondence  matching,  and 
correcting  the  navigation  state.  An  overview  of  the 
image-aided  inertial  navigation  algorithm  is  shown 
in  Figure  4. 

Feature  detection  is  the  process  of  automatically  lo¬ 
cating  objects  in  an  image  and  building  some  descrip¬ 
tor  that  allows  the  feature  to  be  located  in  subsequent 
images.  The  optimal  feature  has  three  characteristics: 

•  easy  to  detect  (i.e.,  strong ) 


Figure  4:  Overview  of  image-aided  inertial  algorithm.  In¬ 
ertial  measurements  are  used  to  aid  the  feature  correspon¬ 
dence  search  which,  in  turn,  is  used  to  correct  the  naviga¬ 
tion  state. 

•  unique  (i.e.,  there  exist  significant  differences 
which  make  this  feature  different  from  other  fea¬ 
tures) 

•  ego-motion  invariant  (i.e.,  the  feature  descriptor 
does  not  change  as  a  result  of  camera  motion) 

One  popular  algorithm  that  partially  addresses  these 
issues  is  Lowe's  scale-invariant  feature  transforma¬ 
tion  (SIFT)  [6].  The  SIFT  algorithm  selects  features 
which  have  a  strong  gradient  in  both  x  and  y  direc¬ 
tions.  Once  the  features  are  selected  that  meet  the 
gradient  threshold,  a  128-byte  descriptor  is  calculated 
such  that  it  is  invariant  to  rotation  and  scale  changes. 
Additionally,  the  SIFT  descriptor  has  been  empiri¬ 
cally  shown  to  be  distinct  and  appropriate  for  robust 
feature  matching.  One  drawback  of  the  SIFT  algo¬ 
rithm  is  the  features  are  only  partially  invariant  to 
affine  (i.e.,  perspective)  changes.  The  other  drawback 
of  the  SIFT  algorithm  is  the  computational  complex¬ 
ity  which  can  lead  to  difficulties  implementing  the 
algorithm  in  real-time.  One  solution  to  this  issue  is 
to  leverage  the  use  of  general-purpose  graphical  pro¬ 
cessing  units  (GPGPU)  to  speed  up  the  feature  extrac¬ 
tion  task  [3].  Our  research  has  shown  processing  time 
reductions  of  up  to  30x  when  using  a  GPU  to  extract 
robust  features. 

Once  features  of  interest  have  been  identified,  the 
next  step  is  to  determine  the  correspondence  between 
each  feature  and  a  matching  feature  in  multiple  im¬ 
ages.  When  an  inertial  sensor  is  available,  the  method 
of  employing  stochastic  constraints  becomes  attrac¬ 
tive  [15].  This  technique  is  illustrated  in  Figure  5.  The 
inertial  measurements  are  used  to  statistically  predict 


the  location  of  a  current-image  feature  into  a  future 
image  in  order  to  reduce  the  search  space  and  elimi¬ 
nate  statistically-unlikely  false  matches. 


Figure  5:  Stochastic  feature  projection.  Optical  features  of 
interest  are  mapped  into  future  images  using  inertial  mea¬ 
surements  and  stochastic  projections. 

Once  the  correspondence  has  been  determined  be¬ 
tween  the  predicted  location  of  a  feature  and  the  mea¬ 
sured  location  in  the  current  image,  the  navigation 
state  is  corrected  using  a  Kalman  filter  update.  The 
update  is  calculated  using  the  projected  feature  lo¬ 
cations  in  Eqns.  (8)  and  (9)  with  an  additive,  white 
Gaussian  noise.  The  Jacobian  of  the  nonlinear  mea¬ 
surement  is  calculated  about  the  nominal  trajectory 
in  terms  of  the  system  states  described  previously. 

In  the  next  section,  the  camera  model  is  defined 
mathematically  along  with  the  feature  extraction  and 
tracking  methodology. 

Imaging  Model  and  Updates 

In  addition  to  the  GPS  and  inertial  sensors,  a  digi¬ 
tal  camera  is  used  to  track  features  of  interest.  The 
first  item  of  interest  is  the  definition  of  the  camera 
projection  model.  After  properly  compensating  for 
the  effects  of  nonlinear  distortions  (see  [2]  or  [7]),  the 
camera  is  modeled  as  a  pin-hole  device.  The  pin-hole 
camera  model  is  shown  in  Figure  6.  This  implies  that 
the  location  of  a  feature  on  the  image  plane  is  simply 
the  projection  of  the  true  location  vector  of  the  fea¬ 
ture  expressed  in  the  camera  frame.  Thus,  given  a 
point  source  at  location  sc  the  resulting  location  of  the 
point  source  in  the  image  plane,  relative  to  the  optical 
center  of  the  camera  is  given  by 

s  proj 

where  s\  is  the  distance  of  the  point  source  from  the 


Figure  6:  Camera  projection  model.  The  pinhole  camera 
model  is  represented  by  placing  a  virtual  image  plane  one 
focal  length  in  front  of  the  optical  center. 

optical  center  of  the  camera  in  the  zc  direction.  The 
pinhole  camera  model  is  discussed  in  more  detail 
in  [7], 

Given  the  location  of  a  feature  relative  to  the  naviga¬ 
tion  frame,  (tn),  and  the  current  position  and  orien¬ 
tation  of  the  vehicle,  (pn,  Cjj),  the  line-of-sight  vector 
in  the  camera  frame  can  be  calculated  as 

sc  =  {CnbCbc)T  (t"  -  p")  (9) 

where  Cb  is  the  camera-to-body  frame  direction  co¬ 
sine  matrix  (DCM). 

In  the  next  section,  the  experimental  simulation  and 
flight  test  results  are  presented. 

EXPERIMENT 

The  data  collection  system  consisted  of  a  consumer- 
grade  MEMS  IMU  and  two  digital  cameras.  The  IMU 
was  a  Microbotics  MIDG  consumer-grade  MEMS  unit 
(see  [10])  which  measured  acceleration  and  angular 
rate  at  50  Hz.  The  digital  cameras  were  both  Pix- 
elink  A-741  machine  vision  cameras  which  incorpo¬ 
rated  a  global  shutter  feature  and  a  Firewire  inter¬ 
face.  The  lenses  were  wide-angle  lenses  with  approx¬ 
imately  90  degrees  field  of  view.  The  sensors  were 
mounted  on  an  aluminum  bracket  and  calibrated  us¬ 
ing  procedures  similar  to  those  described  in  [13].  Im¬ 
ages  were  captured  at  approximately  2.5  Hz.  The 
cameras  were  pointed  down  and  forward  at  a  45- 
degree  angle,  respectively.  For  this  paper,  only  the 
downward-pointing  camera  was  used. 

The  data  collection  system  was  mounted  on  a  C-12 
Huron  aircraft,  owned  by  the  USAF  Test  Pilot  School 
(USAF  TPS).  A  number  of  data  collections  were  flown 


Figure  1:  Flight  path  for  Palmdale  data  collection.  The  seg¬ 
ment  used  in  this  research  was  flown  over  a  combination  of 
urban  and  desert  terrain.  Map  imagery:  ©Digital  Globe, 
NextView. 


marks  was  generated  using  a  digital  terrain  elevation 
database.  In  order  to  simulate  the  effects  of  uncer¬ 
tainty  in  the  terrain  model,  a  random  elevation  error 
was  added  to  the  landmark  elevations.  These  sim¬ 
ulated  landmarks  were  then  projected  into  the  im¬ 
age  plane  and  used  to  generate  synthetic  SIFT  fea¬ 
tures,  complete  with  localization  and  a  randomized 
descriptor  component.  This  method  was  chosen  in 
order  to  stimulate  the  feature  tracking  algorithm  as 
accurately  as  possible  without  going  to  the  difficult 
step  of  generating  simulated  images  with  enough  fi¬ 
delity  to  exercise  the  feature  tracking  algorithm.  As  a 
result  the  Monte  Carlo  simulations  should  be  an  op¬ 
timistic  predictor  of  the  true  system  performance  as 
they  correspond  to  a  "best-case"  scenario  where  there 
is  virtually  perfect  feature  tracking  and  a  perfect  cam¬ 
era  distortion  model.  Thus,  when  the  results  are  in¬ 
terpreted  using  this  perspective,  they  can  help  to  val¬ 
idate  the  concept  and  model,  while  serving  as  a  lower 
bound  performance  expectation  for  this  particular  al¬ 
gorithm. 


by  USAF  TPS  in  order  to  present  the  sensor  with  vari¬ 
ations  in  terrain,  airspeed,  and  altitude.  For  this  test, 
a  segment  of  flight  data  was  used  that  consisted  of  a 
combination  of  urban  and  rural  terrain  (see  Figure  7). 
Further  variations  in  terrain  and  feature  quality  will 
be  tested  in  future  efforts.  A  GAINR-Lite  (GLITE) 
Time-Space  Positioning  Information  (TSPI)  unit  was 
mounted  on  the  aircraft  and  served  as  the  position, 
velocity,  and  orientation  truth  source.  The  GLITE 
system  consists  of  a  FIG-1700  tactical-grade  inertial 
sensor  coupled  with  differentially-corrected  pseudo¬ 
range  measurements  from  a  dual-frequency  GPS  re¬ 
ceiver.  The  sensors  are  integrated  using  a  post-flight 
smoother  algorithm. 

Simulation 

A  Monte  Carlo  simulation  was  used  to  evaluate  the 
performance  and  stability  of  the  GPS-aided  inertial 
navigation  algorithm  both  with  and  without  image- 
aiding.  The  simulations  were  performed  using  a  stan¬ 
dard  flight  profile,  based  as  closely  as  possible  to  the 
experimental  flight  data.  The  simulated  trajectory 
was  generated  based  on  a  semi-circular  path  with  no 
overlapping  portions.  The  trajectory  was  flown  at 
approximately  1000  meters  above  relatively  flat  ter¬ 
rain  above  Palmdale,  California.  A  five-minute  seg¬ 
ment  of  the  profile  was  chosen  with  a  combination  of 
straight-and-level  and  turning  portions. 

For  each  Monte  Carlo  simulation,  the  inertial  mea¬ 
surements  and  GPS  measurements  were  generated 
with  random  errors  consistent  with  the  relevant  er¬ 
ror  model.  In  addition,  a  collection  of  ground  land- 


As  mentioned  previously,  the  target  location  error  is 
a  function  of  the  position  and  attitude  errors  of  the 
navigation  system,  camera  boresight  and  uncorrected 
optical  errors,  and  any  errors  in  the  terrain  elevation 
database  used  to  initialize  the  height  estimate  of  the 
landmark.  The  horizontal  target  error  can  be  thus  ap¬ 
proximated  by  expressing  the  horizontal  target  loca¬ 
tion,  yh,  as: 

Dh  =  Ph  +  hv/tand  (10) 

where  ph  represents  the  horizontal  position  of  the  air¬ 
craft,  hv  is  the  height  of  the  aircraft  above  the  terrain, 
and  9  is  the  depression  angle  from  horizontal  to  the 
target.  Linearizing  about  the  mean,  represented  us¬ 
ing  the  overbar  notation,  results  in  the  following  first- 
order  approximation 


Syh  ~  Sph  +  — =6hv - -=S6  (11) 

tan  0  sin  8 

Assuming  independence,  the  horizontal  target  loca¬ 
tion  variance,  cqy ,  is 
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(12) 


Examining  Eq.  12  shows  a  dependence  on  the  posi¬ 
tion  and  attitude  uncertainty,  as  expected.  Thus,  for 
a  given  imaging  scenario,  the  relative  contribution  of 
each  error  source  can  be  determined.  Assuming  one 
meter  of  horizontal  position  error  and  three  meters 
of  vertical  position  error,  the  expected  target  location 
accuracy  is  a  function  of  the  height  above  terrain,  ele¬ 
vation  angle,  and  attitude  errors.  The  resulting  target 
location  accuracy  given  variations  of  these  parame¬ 
ters  is  shown  in  Figure  8. 


Sensor  Attitude  Error  Standard  Deviation  (rad) 


Figure  8:  Predicted  Horizontal  Target  Location  Error.  The 
predicted  horizontal  target  location  error  (TLE)  is  shown 
for  depression  angles  of  30,  60,  and  90  degrees.  The  effects 
of  attitude  accuracy  and  height  above  terrain  demonstrates 
the  critical  importance  of  reducing  attitude  errors  in  order 
to  improve  target  location  accuracy.  The  figures  assume 
a  1-meter  horizontal  and  3-meter  vertical  position  error  of 
the  aircraft. 


Figure  9:  Simulated  attitude  errors  for  consumer-grade 
GPS/IMU  integration.  As  expected,  the  Monte  Carlo  sim¬ 
ulation  predicts  relatively  large  heading  errors.  These  er¬ 
rors  would  have  a  significant  effect  on  target  location  accu¬ 
racy. 


Simulation  Results 

The  simulation  results  for  the  baseline  GPS/IMU  (no 
image  aiding)  case  illustrates  the  difficulty  in  ob¬ 
taining  a  high-quality  target  location  from  a  UAV 
equipped  with  a  low-cost  inertial  sensor.  As  shown 
in  Figure  9,  the  attitude  errors,  especially  in  heading, 
have  standard  deviations  on  the  order  of  100  mrad. 
This  results  in  a  high  target  location  errors,  especially 
at  longer  slant  ranges. 

Incorporating  the  automated  image-based  feedback 
improves  the  attitude  errors  greatly.  The  resulting 
simulated  attitude  errors  are  shown  in  Figure  10.  The 
standard  deviation  are  on  the  order  of  5  mrad,  which 
is  a  significant  improvement. 

Flight  Test 

The  image-aided  GPS/IMU  algorithm  was  applied 
to  a  five-minute  segment  of  the  Palmdale,  CA  flight 
profile.  The  flight  profile  consisted  of  a  semi-circular 
path,  flown  over  a  combination  of  urban  and  desert 
terrain.  A  sample  image  from  the  image-aiding  al¬ 
gorithm  illustrates  the  typical  terrain  observed  by  the 
sensor  as  well  as  a  depiction  of  the  camera  field-of- 
view  overlayed  on  the  moving-map  display  (see  Fig¬ 
ure  11).  For  purposes  of  comparison,  the  navigation 
solution  was  calculated  using  the  baseline  GPS/IMU 


Figure  10:  Simulated  attitude  errors  for  consumer-grade 
GPS/IMU  integration  with  image  aiding  using  landmarks 
of  opportunity.  Incorporating  the  image-based  measure¬ 
ments  significantly  improves  the  predicted  attitude  errors, 
resulting  in  an  improvement  in  target  location  accuracy. 


Figure  11:  Sample  feature  track  display  from  the  Palmdale 
data  collection.  The  currently-tracked  features  are  depicted 
with  red  '+'  symbols.  Each  feature  is  surrounded  by  a  pro¬ 
jection  of  the  two-sigma  error  ellipse.  In  the  right  pane,  the 
current  features,  quality,  and  field-of-view  are  overlayed  on 
a  moving-map  display.  Map  imagery:  ©Digital  Globe, 
NextView. 


and  using  the  image-aided  GPS/IMU.  The  position 
and  attitude  errors  are  compared  in  Figures  12  and  13, 
respectively.  A  detailed  view  of  the  experimental  at¬ 
titude  errors  is  shown  in  Figure  14.  Although  the  de¬ 
viations  are  larger  than  predicted  by  simulation,  the 
estimate  appears  to  be  consistent  with  the  filter's  pre¬ 
dicted  one-sigma  bounds. 

As  expected,  the  automatic  image  aiding  significantly 
improves  the  attitude  errors  of  the  system.  In  addi¬ 
tion,  the  position  errors  are  consistent,  which  indi¬ 
cates  the  GPS  measurements  are  dominating  the  po¬ 
sitioning  performance  and  that  the  image  updates  do 
not  degrade  this  capability. 

CONCLUSIONS 

In  this  article,  an  algorithm  was  presented  that 
improves  the  target  location  accuracy  of  a  UAV 
equipped  with  a  low-cost  GPS/IMU  and  imaging  sys¬ 
tem.  The  method  is  implemented  recursively  for  on¬ 
line  operation  and  only  requires  a  terrain  database. 
No  a  prori  imagery  is  required  over  the  area  of  oper¬ 
ations.  The  system  automatically  selects  and  tracks 
stationary  features  in  the  field-of-view  and  uses  these 
tracks  to  update  the  navigation  state.  The  system  was 
shown  using  a  combination  of  simulation  and  flight 
test  data  to  improve  the  attitude  accuracy  by  an  or¬ 
der  of  magnitude.  This  results  in  a  corresponding  im¬ 
provement  in  target  location  accuracy. 

The  next  step  of  this  project  is  to  build  a  smaller  sen- 
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Figure  12:  Position  errors  for  Palmdale  flight  test.  The  po¬ 
sition  errors  were  calculated  for  the  GPS/IMU  system  with 
(represented  by  the  dashed  green  trace )  and  without  image 
aiding  (represented  by  the  solid  blue  trace).  The  dashed 
red  lines  indicate  the  one-sigma  uncertainty  bounds  of  the 
EKF  for  the  non-image  aided  case.  Note  the  image  aiding 
does  not  significantly  affect  the  positioning  accuracy  of  the 
system. 


Figure  13:  Attitude  errors  for  Palmdale  flight  test.  The  at¬ 
titude  errors  were  calculated  for  the  GPS/IMU  system  with 
(represented  by  the  dashed  green  trace)  and  without  image 
aiding  (represented  by  the  solid  blue  trace).  The  dashed  red 
lines  indicate  the  one-sigma  uncertainty  bounds  of  the  EKF 
for  the  non-image  aided  case.  As  predicted  by  simulation, 
the  attitude  errors  are  significantly  improved  by  incorpo¬ 
rating  automatic  image  aiding  to  the  GPS/IMU  system. 


Time  (s) 

Figure  14:  Image-aided  attitude  errors  for  Palmdale  flight 
test  (detail).  The  attitude  errors  calcidated  using  the 
image-aided  GPS/IMU  system  show  significant  improve¬ 
ment  over  the  non-image  aided  results.  The  estimates  are 
reasonably  consistent  with  the  EKF  predicted  one-sigma 
bounds  (red  dashed  lines). 

sor  suitable  for  incorporation  in  our  micro-UAV  plat¬ 
form. 
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